package ftn.gitis.lidar.filtering;

import delaunay_triangulation.Point_dt;
import delaunay_triangulation.Triangle_dt;

public class LidarPoint extends Point_dt{

	/** 
	 * Point class. Possible values: 0-Unclassified, 1-Ground, ...
	 */
	private int classification;

	/**
	 * Default constructor. 
	 */
	public LidarPoint() {
		super();
		classification = 0;
	}
	
	/**
	 * Constructs 3D point and sets its class to "Unclassified".
	 * @param x
	 * @param y
	 * @param z
	 */
	public LidarPoint(double x, double y, double z) {
		super(x, y, z);
		classification = 0;
	}

	/**
	 * Constructs 3D point and sets its class.
	 * @param x
	 * @param y
	 * @param z
	 * @param pclass
	 */
	public LidarPoint(double x, double y, double z, int classification) {
		super(x, y, z);
		this.classification = classification;
	}
	
	/**
	 * Returns class of this point.
	 * @return class of this point
	 */
	public int getClassification() {
		return classification;
	}

	/**
	 * Sets new classification of this point.
	 * @param classification
	 */
	public void setClassification(int classification) {
		this.classification = classification;
	}
	
	/**
	 * Returns shortest distance of this point to triangle this point falls in.
	 * @param triangle
	 * @return
	 */
	public double getDistanceFromTriangle(Triangle_dt triangle) {
		if(triangle.isHalfplane()) 
			triangle = triangle.next_12();
		double[] plane = TriangleUtils.getTrianglePlane(triangle);
		double result = (plane[0] * this.x() + plane[1] * this.y() + plane[2] * this.z() + plane[3])
						/ Math.sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);
		return result;
	}
	
	/**
	 * Calculates angles from this point to each triangle vertex and returns largest of them.
	 * @param triangle 
	 * @return largest angle from this point to triangle vertices.
	 */
	public double getMaxAngleToTriangle(Triangle_dt triangle) {
		Triangle_dt t = triangle;
		double angle1 = 0.0D;
		double angle2 = 0.0D; 
		double angle3 = 0.0D;
		double result;
		boolean isHalfplane = false;
		
		double[] plane;
		double[] line1 = TriangleUtils.getLineThrough2Points(this, triangle.p1());
		double[] line2 = TriangleUtils.getLineThrough2Points(this, triangle.p2());		
		double[] line3; 
		
		if(triangle.isHalfplane()) {
			isHalfplane = true;
			plane = TriangleUtils.getTrianglePlane(triangle.next_12());
		} else 
			plane = TriangleUtils.getTrianglePlane(triangle);
		
		angle1 = - Math.asin((plane[0]*line1[0] + plane[1]*line1[1] + plane[2]*line1[2]) // - to make angle positive if point is above or negative if below triangle plane
				/ (Math.sqrt(plane[0]*plane[0] + plane[1]*plane[1] +plane[2]*plane[2]) 
				* Math.sqrt(line1[0]*line1[0] + line1[1]*line1[1] + line1[2]*line1[2]))); 
		
		result = angle1;
		
		angle2 = - Math.asin((plane[0]*line2[0] + plane[1]*line2[1] + plane[2]*line2[2]) // - to make angle positive if point is above or negative if below triangle plane
				/ (Math.sqrt(plane[0]*plane[0] + plane[1]*plane[1] +plane[2]*plane[2]) 
				* Math.sqrt(line2[0]*line2[0] + line2[1]*line2[1] + line2[2]*line2[2])));
		
		if(Math.abs(angle2) > result) result = angle2;
		
		if (!isHalfplane) {
			line3 = TriangleUtils.getLineThrough2Points(this, triangle.p3());
			
			angle3 = - Math.asin((plane[0]*line3[0] + plane[1]*line3[1] + plane[2]*line3[2]) // - to make angle positive if point is above or negative if below triangle plane
					/ (Math.sqrt(plane[0]*plane[0] + plane[1]*plane[1] +plane[2]*plane[2]) 
					* Math.sqrt(line3[0]*line3[0] + line3[1]*line3[1] + line3[2]*line3[2])));
		
			if(Math.abs(angle3) > result) result = angle3;
		} 		
		
		return result;
	}
	
	public String toString() {
		return new String(this.x() + ", " + this.y() + ", " + this.z());
	}
	/**
	 * Classify point as ground if satisfies iteration criteria. 
	 * @param triangulation the TIN that represents ground surface.
	 */
	/*public boolean onGround(Delaunay_Triangulation triangulation, ClassificationParameters params) {
		Triangle_dt triangle;
		double[] angles;
		double distance;
		boolean result = false;
		/* Find the triangle point falls on */
		/*triangle = triangulation.find(this);
		if(!triangle.isHalfplane()) {
			angles = TriangleUtils.getAnglesToTriangleVertices(this, triangle);
			distance = TriangleUtils.getDistanceFromTriangle(this, triangle);
			if((Math.abs(distance) < params.getIterationDistance()) && (Math.abs(angles[0]) < params.getIterationAngle()) 
					&& (Math.abs(angles[1]) < params.getIterationAngle()) && (Math.abs(angles[2]) < params.getIterationAngle())) {
				classification = 1;
				result = true;
			}
		}
		return result;
	}*/
	
	
}
